/** \file
 * \brief Example code for Simple Open EtherCAT master
 *
 * Usage : simple_test [ifname1]
 * ifname is NIC interface, f.e. eth0
 *
 * This is a minimal test.
 *
 * (c)Arthur Ketels 2010 - 2011
 */

#include <stdio.h>
#include <string.h>
#include <sys/time.h>
#include <unistd.h>
#include <pthread.h>

#include "ethercattype.h"
#include "nicdrv.h"
#include "ethercatbase.h"
#include "ethercatmain.h"
#include "ethercatdc.h"
#include "ethercatcoe.h"
#include "ethercatfoe.h"
#include "ethercatconfig.h"
#include "ethercatprint.h"

#include <time.h>
#include <stdint.h>
#define LOOP_PERIOD_NS 1000000
#define OFFSET_NS       100000

struct timespec startTime = {
	0,
	0
};

struct timespec wakeupTime = {
	0,
	0
};

struct timespec curTime = {
	0,
	0
};

struct timespec sleeptime = {
	0,
	0
};
int error;

int diff(struct timespec newtime, struct timespec oldtime) {
	return 1000000000*(newtime.tv_sec - oldtime.tv_sec) + newtime.tv_nsec - oldtime.tv_nsec;
}

void wait_for_next_cycle() {
	clock_gettime(CLOCK_MONOTONIC, &curTime);
	sleeptime.tv_nsec = LOOP_PERIOD_NS - curTime.tv_nsec % LOOP_PERIOD_NS;
	nanosleep(&sleeptime, NULL);
}

void wait_for_next_cycle_dc(int64 DCtime) {
	clock_gettime(CLOCK_MONOTONIC, &curTime);
	sleeptime.tv_nsec = (LOOP_PERIOD_NS - ((DCtime + LOOP_PERIOD_NS/2) % LOOP_PERIOD_NS
	                                       - (LOOP_PERIOD_NS/2)) - diff(curTime, wakeupTime)) % LOOP_PERIOD_NS;
	nanosleep(&sleeptime, NULL);
	clock_gettime(CLOCK_MONOTONIC, &wakeupTime);
}

#define EC_TIMEOUTMON          500

char IOmap[4096];
pthread_t thread1;
int expectedWKC;
boolean needlf;
volatile int wkc;
boolean inOP;
uint8 currentgroup = 0;

void simpletest(char *ifname)
{
	int i, j, oloop, iloop, wkc_count, chk;
	needlf = FALSE;
	inOP = FALSE;

	printf("Starting simple test\n");

	/* initialise SOEM, bind socket to ifname */
	if (ec_init(ifname))
	{
		printf("ec_init on %s succeeded.\n",ifname);
		/* find and auto-config slaves */

		if ( ec_config_init(FALSE) > 0 )
		{
			printf("%d slaves found and configured.\n",ec_slavecount);

			ec_config_map(IOmap);

			ec_configdc();
			clock_gettime(CLOCK_MONOTONIC, &startTime);

			printf("Slaves mapped, state to SAFE_OP.\n");
			/* wait for all slaves to reach SAFE_OP state */
			ec_statecheck(0, EC_STATE_SAFE_OP,  EC_TIMEOUTSTATE * 4);

			oloop = ec_slave[0].Obytes;
			if ((oloop == 0) && (ec_slave[0].Obits > 0)) oloop = 1;
			if (oloop > 8) oloop = 8;
			iloop = ec_slave[0].Ibytes;
			if ((iloop == 0) && (ec_slave[0].Ibits > 0)) iloop = 1;
			if (iloop > 8) iloop = 8;

			printf("segments : %d : %d %d %d %d\n",ec_group[0].nsegments,ec_group[0].IOsegment[0],ec_group[0].IOsegment[1],ec_group[0].IOsegment[2],ec_group[0].IOsegment[3]);

			printf("Request operational state for all slaves\n");
			expectedWKC = (ec_group[0].outputsWKC * 2) + ec_group[0].inputsWKC;
			printf("Calculated workcounter %d\n", expectedWKC);
			ec_slave[0].state = EC_STATE_OPERATIONAL;
			/* send one valid process data to make outputs in slaves happy*/
			ec_send_processdata();
			ec_receive_processdata(EC_TIMEOUTRET);
			/* request OP state for all slaves */
			ec_writestate(0);
			chk = 40;
			/* wait for all slaves to reach OP state */
			do
			{
				ec_send_processdata();
				ec_receive_processdata(EC_TIMEOUTRET);
				ec_statecheck(0, EC_STATE_OPERATIONAL, 50000);
			}
			while (chk-- && (ec_slave[0].state != EC_STATE_OPERATIONAL));
			if (ec_slave[0].state == EC_STATE_OPERATIONAL)
			{
				printf("Operational state reached for all slaves.\n");
				wkc_count = 0;
				inOP = TRUE;
				ec_dcsync0(1, TRUE, 1000000, 0);
				//ec_dcsync0(2, TRUE, 1000000, 0);
				usleep(100000);
				/* cyclic loop */
				uint8_t num = 0;
				for(i = 1; i <= 1000000; i++)
				{
					if (num != ec_slave[1].outputs[0]) printf("%u\t%u\n", num, ec_slave[1].outputs[0]);
					ec_slave[1].outputs[0] = ++num;
					
					ec_send_processdata();
					wkc = ec_receive_processdata(500);
					if (ec_slave[1].outputs[0] != ec_slave[1].inputs[0])
						printf("Whoa.\n");
					//ec_slave[1].outputs[0] = ec_slave[1].inputs[0] + 1;
					ec_send_processdata();
					printf("%u\n", ec_slave[1].outputs[0]);
					wkc = ec_receive_processdata(500);
					
					clock_gettime(CLOCK_MONOTONIC, &curTime);
					//printf("%u, %u\n", wkc, ec_FPWRw(1, 0x912, /*(1000000000*(curTime.tv_sec - startTime.tv_sec) + curTime.tv_nsec - startTime.tv_nsec) % 1 << 31*/ 0x0016, 500));

					if(wkc >= expectedWKC)
					{
						//printf("Processdata cycle %4d, WKC %d , O:", i, wkc);

						for(j = 0; j < oloop; j++)
						{
							//printf(" %2.2x", *(ec_slave[0].outputs + j));
						}

						//printf(" I:");
						for(j = 0; j < iloop; j++)
						{
							//printf(" %2.2x", *(ec_slave[0].inputs + j));
						}
						//printf(" T:%lld\n",ec_DCtime);
						needlf = TRUE;
					}
					wait_for_next_cycle();
				}
				inOP = FALSE;
			}
			else
			{
				printf("Not all slaves reached operational state.\n");
				ec_readstate();
				for(i = 1; i<=ec_slavecount; i++)
				{
					if(ec_slave[i].state != EC_STATE_OPERATIONAL)
					{
						printf("Slave %d State=0x%2.2x StatusCode=0x%4.4x : %s\n",
						       i, ec_slave[i].state, ec_slave[i].ALstatuscode,
						       ec_ALstatuscode2string(ec_slave[i].ALstatuscode));
					}
				}
			}
			printf("\nRequest init state for all slaves\n");
			ec_slave[0].state = EC_STATE_INIT;
			/* request INIT state for all slaves */
			ec_writestate(0);
		}
		else
		{
			printf("No slaves found!\n");
		}
		printf("End simple test, close socket\n");
		/* stop SOEM, close socket */
		ec_close();
	}
	else
	{
		printf("No socket connection on %s\nExcecute as root\n",ifname);
	}
}

void ecatcheck( void *ptr )
{
	int slave;

	while(1)
	{
		if( inOP && ((wkc < expectedWKC) || ec_group[currentgroup].docheckstate))
		{
			if (needlf)
			{
				needlf = FALSE;
				printf("\n");
			}
			/* one or more slaves are not responding */
			ec_group[currentgroup].docheckstate = FALSE;
			ec_readstate();
			for (slave = 1; slave <= ec_slavecount; slave++)
			{
				if ((ec_slave[slave].group == currentgroup) && (ec_slave[slave].state != EC_STATE_OPERATIONAL))
				{
					ec_group[currentgroup].docheckstate = TRUE;
					if (ec_slave[slave].state == (EC_STATE_SAFE_OP + EC_STATE_ERROR))
					{
						printf("ERROR : slave %d is in SAFE_OP + ERROR, attempting ack.\n", slave);
						ec_slave[slave].state = (EC_STATE_SAFE_OP + EC_STATE_ACK);
						ec_writestate(slave);
					}
					else if(ec_slave[slave].state == EC_STATE_SAFE_OP)
					{
						printf("WARNING : slave %d is in SAFE_OP, change to OPERATIONAL.\n", slave);
						ec_slave[slave].state = EC_STATE_OPERATIONAL;
						ec_writestate(slave);
					}
					else if(ec_slave[slave].state > 0)
					{
						if (ec_reconfig_slave(slave, EC_TIMEOUTMON))
						{
							ec_slave[slave].islost = FALSE;
							printf("MESSAGE : slave %d reconfigured\n",slave);
						}
					}
					else if(!ec_slave[slave].islost)
					{
						/* re-check state */
						ec_statecheck(slave, EC_STATE_OPERATIONAL, EC_TIMEOUTRET);
						if (!ec_slave[slave].state)
						{
							ec_slave[slave].islost = TRUE;
							printf("ERROR : slave %d lost\n",slave);
						}
					}
				}
				if (ec_slave[slave].islost)
				{
					if(!ec_slave[slave].state)
					{
						if (ec_recover_slave(slave, EC_TIMEOUTMON))
						{
							ec_slave[slave].islost = FALSE;
							printf("MESSAGE : slave %d recovered\n",slave);
						}
					}
					else
					{
						ec_slave[slave].islost = FALSE;
						printf("MESSAGE : slave %d found\n",slave);
					}
				}
			}
			//if(!ec_group[currentgroup].docheckstate)
			//	printf("OK : all slaves resumed OPERATIONAL.\n");
		}
		usleep(10000);
	}
}

int main(int argc, char *argv[])
{
	int iret1;
	printf("SOEM (Simple Open EtherCAT Master)\nSimple test\n");

	if (argc > 1)
	{
		/* create thread to handle slave error handling in OP */
		iret1 = pthread_create( &thread1, NULL, (void *) &ecatcheck, (void*) &ctime);
		/* start cyclic part */
		simpletest(argv[1]);
	}
	else
	{
		printf("Usage: simple_test ifname1\nifname = eth0 for example\n");
	}

	printf("End program\n");
	return (0);
}
